package kit.edu.lego.gruppe3.levels.line;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.actions.Orientation;
import kit.edu.lego.gruppe3.bluetooth.control.GateCommon;
import kit.edu.lego.gruppe3.bluetooth.control.GateControl;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.sensors.GroundType;
import kit.edu.lego.gruppe3.sensors.Scanner;
import lejos.nxt.LightSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.*;
import kit.edu.lego.gruppe3.LegoMain;

public class LineEndReached implements Behavior {

	private DifferentialPilot robot = ComponentsRepository.getInstance()
			.getRobotPilot();
	LightSensor lightSensor = ComponentsRepository.getInstance()
			.getLightSensor();

	@Override
	public void action() {

		robot.setRotateSpeed(50);
		robot.rotate(-130, true);
		while (robot.isMoving())
			if (lightSensor.readValue() > ComponentsRepository.LIGHT_TRASHOLD) {
				robot.stop();
				ComponentsRepository.endOfLine = false;
				return;
			}
		robot.rotate(+260, true);
		while (robot.isMoving())
			if (lightSensor.readValue() > ComponentsRepository.LIGHT_TRASHOLD) {
				robot.stop();
				ComponentsRepository.endOfLine = false;
				return;
			}
		robot.rotate(-130, true);
		while (robot.isMoving())
			if (lightSensor.readValue() > ComponentsRepository.LIGHT_TRASHOLD) {
				robot.stop();
				ComponentsRepository.endOfLine = false;
				return;
			}
		if (LineSolver.getState() == 0) {
			robot.setTravelSpeed(4);
			robot.forward();
			while (robot.isMoving())
				if (lightSensor.readValue() > ComponentsRepository.LIGHT_TRASHOLD)
					robot.stop();
			robot.forward();
			while (robot.isMoving())
				if (Scanner.getGroundTypeAt(0, true) != GroundType.Wood)
					robot.stop();
			LegoMain.startRollBridge();
		}
		if (LineSolver.getState() == 1) {
			LegoMain.startFirstDoorSolver();
		}
		if (LineSolver.getState() == 2) {
			robot.setTravelSpeed(4);
			robot.forward();
			while (robot.isMoving())
				if (lightSensor.readValue() > ComponentsRepository.LIGHT_TRASHOLD)
					robot.stop();
			Orientation.orientAtType(GroundType.Line);
			LegoMain.startFallingBridge();
		}
		if (LineSolver.getState() == 3) {
			robot.setTravelSpeed(4);
			robot.forward();
			while (robot.isMoving())
				if (lightSensor.readValue() > ComponentsRepository.LIGHT_TRASHOLD)
					robot.stop();
			LegoMain.startRotatingBridge();
		}

		if (LineSolver.getState() == 4)
		{
			GateControl gateControl = new GateControl();
			
			while ( !gateControl.connectionToGateSuccessful(GateCommon.GATE_2));
			
			if (gateControl.openGate()){
				// gate open
			}
			gateControl.disconnectFromGate();
			
			LegoMain.startLabyrinth();
		}

	}

	@Override
	public void suppress() {

	}

	@Override
	public boolean takeControl() {
		return LevelSelector.getInstance().isLine()
				&& !ComponentsRepository.evadeObstacle
				&& lightSensor.readValue() < ComponentsRepository.LIGHT_TRASHOLD
				&& ComponentsRepository.endOfLine;
	}

}
